/*
 * udsv.c
 *
 *  Created on: 2021年2月18日
 *      Author: taoyu
 */

#include <stdint.h>
#include "uds_types.h"
#include "udsServer_settings.h"
#include "udsServer_cfg.h"
#include "udsServer_api.h"
#include "udsServer_port.h"

typedef struct
{
	uint8_t rxMsgData[UDS_MESSAGE_BUFFER_SIZE];
	uint8_t rxMsgLength;

	uint8_t txMsgData[UDS_MESSAGE_BUFFER_SIZE];
	uint8_t txMsgLength;

	// request analysis
	UDS_SID sid;			// service id
	uint8_t rctp;				// routine control type
	uint16_t did;				// data id
	uint16_t rid;				// routine id

	// only for read/write memory by address
	uint8_t addressAndLengthFormat;
	uint8_t sizeOfMemoryAddress;
	uint8_t sizeOfMemorySize;
	uint32_t memoryAddress;
	uint32_t memorySize;

	// only for IO Control
	UDS_IOCP ioControlParameter;
}
UdsServerDataType;

UdsServerDataType udsServerData;

static void udsServer_requestProcess(void);
static void udsServer_sendResponse(UDS_NRC nrc);

static void udsServer_readMemoryByAddress(void);
static void udsServer_writeMemoryByAddress(void);
static void udsServer_ioControl(void);
static void udsServer_readDataByID(void);
static void udsServer_writeDataByID(void);
static void udsServer_routineControl(void);

void udsServer_pport_init(void)
{
}

void udsServer_pport_task1ms(void)
{
	// receive message
	udsServerData.rxMsgLength = udsServer_rport_receive(udsServerData.rxMsgData, sizeof(udsServerData.rxMsgData));
	// when the state change to busy
	if(udsServerData.rxMsgLength>0)
	{
		// process the received message
		udsServer_requestProcess();
	}
}

// process the received message
static void udsServer_requestProcess(void)
{
	// get service ID
	udsServerData.sid = (UDS_SID)udsServerData.rxMsgData[0];

	// routine control type
	udsServerData.rctp = (UDS_RCTP)udsServerData.rxMsgData[1];

	// in many cases the next 2 bytes are data id
	udsServerData.did = udsServerData.rxMsgData[1];
	udsServerData.did <<= 8;
	udsServerData.did |= udsServerData.rxMsgData[2];

	// routine control id
	udsServerData.rid = udsServerData.rxMsgData[2];
	udsServerData.rid <<= 8;
	udsServerData.rid |= udsServerData.rxMsgData[3];

	// switch services
	switch( udsServerData.sid )
	{
		/* io control by id */
		case	UDS_SID_IO_CONTROL:
			udsServer_ioControl();
			break;

		/* read memory by address */
		case UDS_SID_READ_MEM_BY_ADDR:
			udsServer_readMemoryByAddress();
			break;

		/* write memory by address */
		case UDS_SID_WRITE_MEM_BY_ADDR:
			udsServer_writeMemoryByAddress();
			break;

		/* read data by ID */
		case	UDS_SID_READ_DATA_BY_ID:
			udsServer_readDataByID();
			break;

		/* write data by ID */
		case	UDS_SID_WRITE_DATA_BY_ID:
			udsServer_writeDataByID();
			break;

			/* routine control by id */
		case	UDS_SID_ROUTINE_CONTROL:
			udsServer_routineControl();
			break;

		default:
			break;
	}
}

static void udsServer_sendResponse(UDS_NRC nrc)
{
	if(nrc==UDS_NRC_OK)
	{
		udsServerData.txMsgData[0] |= 0x40;
		udsServer_rport_send(udsServerData.txMsgData, udsServerData.txMsgLength);
	}
	else
	{
		udsServerData.txMsgData[0] = 0x7F;
		udsServerData.txMsgData[1] = (uint8_t)udsServerData.sid;
		udsServerData.txMsgData[2] = (uint8_t)nrc;
		udsServerData.txMsgLength = 3;
		udsServer_rport_send(udsServerData.txMsgData, udsServerData.txMsgLength);
	}
}

// calculate memory address and size for read/write memory by address
static void udsServer_calculateMemoryAddressAndSize(void)
{
	uint8_t i;
	uint32_t tmp;

	// memory address
	tmp = udsServerData.rxMsgData[2];
	for(i=1;i<udsServerData.sizeOfMemoryAddress;i++)
	{
		tmp <<= 8;
		tmp |= udsServerData.rxMsgData[2+i];
	}
	udsServerData.memoryAddress = tmp;

	// memory size
	tmp = udsServerData.rxMsgData[2+udsServerData.sizeOfMemoryAddress];
	for(i=1;i<udsServerData.sizeOfMemorySize;i++)
	{
		tmp <<= 8;
		tmp |= udsServerData.rxMsgData[2+udsServerData.sizeOfMemoryAddress+i];
	}
	udsServerData.memorySize = tmp;
}

static void udsServer_readMemoryByAddress(void)
{
	// request format: RMBA(23) ALFID MA MS

	// process address and length
	udsServerData.addressAndLengthFormat = udsServerData.rxMsgData[1];
	udsServerData.sizeOfMemoryAddress = udsServerData.addressAndLengthFormat & 0x0F;
	udsServerData.sizeOfMemorySize = udsServerData.addressAndLengthFormat >> 4;

	// check request length, at least there should be 4 bytes: 23 ALFID MA MS
	if( udsServerData.rxMsgLength<4 )
	{
		udsServer_sendResponse(UDS_NRC_INCORRECT_MESSAGE_LENGTH_OR_INVALID_FORMAT);
	}
	// check request length again, the size of address or size should not be 0
	else if( !udsServerData.sizeOfMemoryAddress || !udsServerData.sizeOfMemorySize)
	{
		udsServer_sendResponse(UDS_NRC_INCORRECT_MESSAGE_LENGTH_OR_INVALID_FORMAT);
	}
	// check request length again, the format should match with the address data
	else if( udsServerData.rxMsgLength!= 2+udsServerData.sizeOfMemoryAddress+udsServerData.sizeOfMemorySize)
	{
		udsServer_sendResponse(UDS_NRC_INCORRECT_MESSAGE_LENGTH_OR_INVALID_FORMAT);
	}
	// check the address and length plausibility
	else
	{
		// calculate the requested memory address and size
		udsServer_calculateMemoryAddressAndSize();

		// check plausibility MIN
		if( udsServerData.memoryAddress < UDS_MEMORY_ADDRESS_MIN )
		{
			udsServer_sendResponse(UDS_NRC_REQUEST_OUT_OF_RANGE);
		}
		// check plausibility MAX
		else if( udsServerData.memoryAddress+udsServerData.memorySize > UDS_MEMORY_ADDRESS_MAX )
		{
			udsServer_sendResponse(UDS_NRC_REQUEST_OUT_OF_RANGE);
		}
		// address and length plausibility pass
		else
		{
			// response data length
			udsServerData.txMsgLength = 1+udsServerData.memorySize;

			// check buffer availability, response format : SID Data ...
			if(udsServerData.txMsgLength <= sizeof(udsServerData.txMsgData))
			{
				uint8_t i;
				uint8_t * pSrc = (uint8_t*) udsServerData.memoryAddress;
				uint8_t * pDst = &udsServerData.txMsgData[1];
				for(i=0;i<udsServerData.memorySize;i++) pDst[i] = pSrc[i];
				udsServerData.txMsgData[0] = 0x40+UDS_SID_READ_MEM_BY_ADDR;
				udsServer_sendResponse(UDS_NRC_OK);
			}
			// buffer not available
			else
			{
				udsServer_sendResponse(UDS_NRC_CONDITION_NOT_CORRECT);
			}
		}
	}
}

static void udsServer_writeMemoryByAddress(void)
{
	// request format: WMBA(3D) ALFID MA MS Data ...

	// process address and length
	udsServerData.addressAndLengthFormat = udsServerData.rxMsgData[1];
	udsServerData.sizeOfMemoryAddress = udsServerData.addressAndLengthFormat & 0x0F;
	udsServerData.sizeOfMemorySize = udsServerData.addressAndLengthFormat >> 4;

	// check request length, at least there should be 5 bytes
	if( udsServerData.rxMsgLength<5 )
	{
		udsServer_sendResponse(UDS_NRC_INCORRECT_MESSAGE_LENGTH_OR_INVALID_FORMAT);
	}
	// check request length again, the size of address or size should not be 0
	else if( !udsServerData.sizeOfMemoryAddress || !udsServerData.sizeOfMemorySize)
	{
		udsServer_sendResponse(UDS_NRC_INCORRECT_MESSAGE_LENGTH_OR_INVALID_FORMAT);
	}
	// check the address and length plausibility
	else
	{
		// calculate the requested memory address and size
		udsServer_calculateMemoryAddressAndSize();

		// check request length again, the format should match with the address/size data
		if( udsServerData.rxMsgLength!= 2+udsServerData.sizeOfMemoryAddress+udsServerData.sizeOfMemorySize+udsServerData.memorySize)
		{
			udsServer_sendResponse(UDS_NRC_INCORRECT_MESSAGE_LENGTH_OR_INVALID_FORMAT);
		}
		// check plausibility MIN
		else if( udsServerData.memoryAddress < UDS_MEMORY_ADDRESS_MIN )
		{
			udsServer_sendResponse(UDS_NRC_REQUEST_OUT_OF_RANGE);
		}
		// check plausibility MAX
		else if( udsServerData.memoryAddress+udsServerData.memorySize > UDS_MEMORY_ADDRESS_MAX )
		{
			udsServer_sendResponse(UDS_NRC_REQUEST_OUT_OF_RANGE);
		}
		// address and length plausibility pass
		else
		{
			// write the data to the address
			uint8_t i;
			uint8_t * pDst = (uint8_t*) udsServerData.memoryAddress;
			uint8_t * pSrc = &udsServerData.rxMsgData[2+udsServerData.sizeOfMemoryAddress+udsServerData.sizeOfMemorySize];
			for(i=0;i<udsServerData.memorySize;i++) pDst[i] = pSrc[i];

			// prepare response message: SID(7D) ALFID MA MS
			udsServerData.txMsgLength = 2+udsServerData.sizeOfMemoryAddress+udsServerData.sizeOfMemorySize;
			udsServerData.txMsgData[0] = 0x40+UDS_SID_WRITE_MEM_BY_ADDR;
			for(i=1;i<udsServerData.txMsgLength;i++) udsServerData.txMsgData[i] = udsServerData.rxMsgData[i];
			udsServer_sendResponse(UDS_NRC_OK);
		}
	}
}

static void udsServer_ioControl(void)
{
	// request format: IOCBI(2F) DIDH DIDL M1 C1 ... C2 ...

	// store the io control parameter (M1)
	udsServerData.ioControlParameter = (UDS_IOCP)udsServerData.rxMsgData[3];

	// check request length, at least 4 bytes: 2F, DID, M1
	if( udsServerData.rxMsgLength<4 )
	{
		udsServer_sendResponse(UDS_NRC_INCORRECT_MESSAGE_LENGTH_OR_INVALID_FORMAT);
	}
	else
	{
		uint8_t i;

		// search for the did
		for(i=0;i<UDS_IOC_DESC_NUMBER;i++)
		{
			// match the did
			if( udsServer_iocDesc[i].did==udsServerData.did)
			{
				break;
			}
			else
			{
				// try next did
			}
		}

		// no did found
		if(i>=UDS_IOC_DESC_NUMBER)
		{
			udsServer_sendResponse(UDS_NRC_REQUEST_OUT_OF_RANGE);
		}
		// did found, check function
		else if( !udsServer_iocDesc[i].ioControl )
		{
			// function is null
			udsServer_sendResponse(UDS_NRC_CONDITION_NOT_CORRECT);
		}
		// function is ok
		else
		{
			// call the interface
			UDS_NRC nrc = udsServer_iocDesc[i].ioControl(udsServerData.did, udsServerData.ioControlParameter, &udsServerData.rxMsgData[4], udsServerData.rxMsgLength-4);
			// prepare positive response
			if( nrc==UDS_NRC_OK )
			{
				for(i=0;i<udsServerData.rxMsgLength;i++) udsServerData.txMsgData[i] = udsServerData.rxMsgData[i];
				udsServerData.txMsgLength = udsServerData.rxMsgLength;
				udsServerData.txMsgData[0] = 0x40 + UDS_SID_IO_CONTROL;
			}
			// send response
			udsServer_sendResponse(nrc);
		}
	}
}

static void udsServer_readDataByID(void)
{
	// request format: RDBI(22) DIDH DIDL

	// check length
	if(udsServerData.rxMsgLength!=3)
	{
		udsServer_sendResponse(UDS_NRC_INCORRECT_MESSAGE_LENGTH_OR_INVALID_FORMAT);
	}
	else
	{
		uint8_t i;

		// search for the did
		for(i=0;i<UDS_RDBI_DESC_NUMBER;i++)
		{
			// match the did
			if( udsServer_rdbiDesc[i].did==udsServerData.did)
			{
				break;
			}
			else
			{
				// try next did
			}
		}

		// no did found
		if(i>=UDS_RDBI_DESC_NUMBER)
		{
			udsServer_sendResponse(UDS_NRC_REQUEST_OUT_OF_RANGE);
		}
		// did found, check function
		else if( !udsServer_rdbiDesc[i].readDataByID )
		{
			// function is null
			udsServer_sendResponse(UDS_NRC_CONDITION_NOT_CORRECT);
		}
		// function is ok
		else
		{
			uint8_t dataLength;
			// call the interface
			UDS_NRC nrc = udsServer_rdbiDesc[i].readDataByID(udsServerData.did, &udsServerData.txMsgData[3], sizeof(udsServerData.txMsgData)-3, &dataLength);
			// prepare positive response
			if( nrc==UDS_NRC_OK )
			{
				// response format: RDBI(62) DIDH DIDL Data ...
				udsServerData.txMsgData[0] = 0x40 + UDS_SID_READ_DATA_BY_ID;
				udsServerData.txMsgData[1] = udsServerData.rxMsgData[1];
				udsServerData.txMsgData[2] = udsServerData.rxMsgData[2];
				udsServerData.txMsgLength = 3+dataLength;
			}
			// send response
			udsServer_sendResponse(nrc);
		}
	}
}

static void udsServer_writeDataByID(void)
{

	// request format: WDBI(2E) DIDH DIDL ...

	// check length
	if(udsServerData.rxMsgLength<=3)
	{
		udsServer_sendResponse(UDS_NRC_INCORRECT_MESSAGE_LENGTH_OR_INVALID_FORMAT);
	}
	else
	{
		uint8_t i;

		// search for the did
		for(i=0;i<UDS_WDBI_DESC_NUMBER;i++)
		{
			// match the did
			if( udsServer_wdbiDesc[i].did==udsServerData.did)
			{
				break;
			}
			else
			{
				// try next did
			}
		}

		// no did found
		if(i>=UDS_WDBI_DESC_NUMBER)
		{
			udsServer_sendResponse(UDS_NRC_REQUEST_OUT_OF_RANGE);
		}
		// did found, check function
		else if( !udsServer_wdbiDesc[i].writeDataByID )
		{
			// function is null
			udsServer_sendResponse(UDS_NRC_CONDITION_NOT_CORRECT);
		}
		// function is ok
		else
		{
			// call the interface
			UDS_NRC nrc = udsServer_wdbiDesc[i].writeDataByID(udsServerData.did, &udsServerData.rxMsgData[3], (int)(udsServerData.rxMsgLength-3));
			// prepare positive response
			if( nrc==UDS_NRC_OK )
			{
				// response format: RDBI(62) DIDH DIDL Data ...
				udsServerData.txMsgData[0] = 0x40 + UDS_SID_WRITE_DATA_BY_ID;
				udsServerData.txMsgData[1] = udsServerData.rxMsgData[1];
				udsServerData.txMsgData[2] = udsServerData.rxMsgData[2];
				udsServerData.txMsgLength = 3;
			}
			// send response
			udsServer_sendResponse(nrc);
		}
	}
}

static void udsServer_routineControl(void)
{
	uint8_t i;
	uint8_t rspLen = 0;

	// request format: RC(31) RCTP RIDH RIDL (Data ...)

	// NRC
	UDS_NRC nrc = UDS_NRC_OK;

	// check length
	if(udsServerData.rxMsgLength<4)
	{
		nrc = UDS_NRC_INCORRECT_MESSAGE_LENGTH_OR_INVALID_FORMAT;
	}

	// find id
	if(UDS_NRC_OK==nrc)
	{
		// search for the routine id
		nrc = UDS_NRC_REQUEST_OUT_OF_RANGE;
		for(i=0;i<UDS_RC_DESC_NUMBER;i++)
		{
			// match the routine id
			if( udsServer_rcDesc[i].rid==udsServerData.rid)
			{
				nrc = UDS_NRC_OK;
				break;
			}
			else
			{
				// try next did
			}
		}
	}

	// sub function
	if(UDS_NRC_OK==nrc)
	{
		// run sub function
		switch (udsServerData.rctp)
		{
			case	UDS_RCTP_RESERVED_0:	// reserved
				// function sub function is not supported
				nrc = UDS_NRC_SUB_FUNCTION_NOT_SUPPORTED;
				break;
			case	UDS_RCTP_START:	// start
				if(udsServer_rcDesc[i].routineControlStart)
				{
					nrc = udsServer_rcDesc[i].routineControlStart(udsServerData.rid, &udsServerData.rxMsgData[4], udsServerData.rxMsgLength-4, &rspLen);
				}
				else
				{
					nrc = UDS_NRC_SUB_FUNCTION_NOT_SUPPORTED;
				}
				break;
			case	UDS_RCTP_STOP:	// stop
				if(udsServer_rcDesc[i].routineControlStop)
				{
					nrc = udsServer_rcDesc[i].routineControlStop(udsServerData.rid, &udsServerData.rxMsgData[4], udsServerData.rxMsgLength-4, &rspLen);
				}
				else
				{
					nrc = UDS_NRC_SUB_FUNCTION_NOT_SUPPORTED;
				}
				break;
			case	UDS_RCTP_REQUEST_RESULT:	// request result
				if(udsServer_rcDesc[i].routineControlRequestResult)
				{
					nrc = udsServer_rcDesc[i].routineControlRequestResult(udsServerData.rid, &udsServerData.rxMsgData[4], udsServerData.rxMsgLength-4, &rspLen);
				}
				else
				{
					nrc = UDS_NRC_SUB_FUNCTION_NOT_SUPPORTED;
				}
				break;
			default:	// reserved
				// function sub function is not supported
				nrc = UDS_NRC_SUB_FUNCTION_NOT_SUPPORTED;
				break;
		}
	}

	// positive response
	if(UDS_NRC_OK==nrc)
	{
		// positive response have same heading
		udsServerData.txMsgLength = 4 + rspLen;
		for(i=0;i<udsServerData.txMsgLength;i++) udsServerData.txMsgData[i] = udsServerData.rxMsgData[i];
	}

	// send response
	udsServer_sendResponse(nrc);
}

